//
//  followTrackHandler.m
//  Legions
//
//  Created by Legions on 06/07/14.
//  Copyright  (c) 2014 RefeSoft. All rights reserved.
//

#import "FollowTrackHandler.h"
#import "GameObjectType.h"
#import "MathUtilities.h"


static const int POINTS_PER_SEC = 80;


@implementation FollowTrackHandler{
    SKSpriteNode<GameObjectType> *gameObject;
    
    NSMutableArray *_wayPoints;
    CGPoint _velocity;
}

-(instancetype) initWithMoveAbleGameObject :(SKSpriteNode<GameObjectType>*) gameObjectToMove{
    self        =   [[FollowTrackHandler alloc] init];
    gameObject  =   gameObjectToMove;
    
    _wayPoints = [NSMutableArray array];
    
    return self;
}

- (void)addPointToMove:(CGPoint)point{
    [_wayPoints addObject:[NSValue valueWithCGPoint:point]];
}

- (void)move:(NSNumber *)dt{
    
    CGPoint currentPosition = gameObject.position;
    CGPoint newPosition;
    
    //1
    if([_wayPoints count] > 0) {
        CGPoint targetPoint = [[_wayPoints firstObject] CGPointValue];
        
        //2 TODO: Add movement logic here
        //1
        CGPoint offset = CGPointMake(targetPoint.x - currentPosition.x, targetPoint.y - currentPosition.y);
        CGFloat length = sqrtf(offset.x * offset.x + offset.y * offset.y);
        CGPoint direction = CGPointMake(offset.x / length, offset.y / length);
        _velocity = CGPointMake(direction.x * POINTS_PER_SEC, direction.y * POINTS_PER_SEC);
        
        //2
        newPosition = CGPointMake(currentPosition.x + _velocity.x * [dt doubleValue],
                                  currentPosition.y + _velocity.y * [dt doubleValue]);
        gameObject.position = newPosition;
        
        //caculate  new angle....
       // CGFloat rad = [MathUtilities angleBetween:gameObject.position and:newPosition];
       // float rotate = M_PI - fmod(gameObject.zRotation, 2*M_PI) - rad; //start angle - current angle - new angle
        
        //if(rotate > M_PI){
        //    rotate = rotate - 2*M_PI;
        //}
        
        //if (rotate < -M_PI) {
        //    rotate = rotate + 2*M_PI;
        //}
        
        gameObject.zRotation = -atan2f(_velocity.x, _velocity.y);;

        
        
        //3
        if(CGRectContainsPoint(gameObject.frame, targetPoint)) {
            [_wayPoints removeObjectAtIndex:0];
        }
    }
}

@end
